#include "ros/ros.h"
#include "can_msgs/Frame.h"
#include "Cybergear_control.hpp"

uint8_t can_id=0;
uint8_t uncalibrated_flag=0;
uint8_t hall_flag=0;
uint8_t magnetic_flag=0;
uint8_t overtemperature_flag=0;
uint8_t overcurrent_flag=0;
uint8_t undervoltage_flag=0;
uint8_t mode_state=0;
float current_angle=0;
float current_angular_velocity=0;
float current_moment=0;
float current_temperature=0;

void do_can_msgs(const can_msgs::Frame::ConstPtr& RxMessage)
{
    ROS_INFO("开始对电机反馈数据进行处理！");
	can_id=(RxMessage->id>>8) & 0xFF;
	uncalibrated_flag=(RxMessage->id>>21) & 0x1;
	hall_flag=(RxMessage->id>>20) & 0x1;
	magnetic_flag=(RxMessage->id>>19) & 0x1;
	overtemperature_flag=(RxMessage->id>>18) & 0x1;
	overcurrent_flag=(RxMessage->id>>17) & 0x1;
	undervoltage_flag=(RxMessage->id>>16) & 0x1;
	mode_state=(RxMessage->id>>22) & 0x3;
	current_angle=data_conversion(RxMessage->data[0]<<8|RxMessage->data[1],angle_min,angle_max);
	current_angular_velocity=data_conversion(RxMessage->data[2]<<8|RxMessage->data[3],angular_velocity_min,angular_velocity_max);
	current_moment=data_conversion(RxMessage->data[4]<<8|RxMessage->data[5],moment_min,moment_max);
	current_temperature=(RxMessage->data[6]<<8|RxMessage->data[7])/10;
	ROS_INFO("current_moment:%f",current_moment);
	if(can_id==1)
	{
		ros::param::set("motor1_angle",current_angle);
		ros::param::set("motor1_angular_velocity",current_angular_velocity);
		ros::param::set("motor1_moment",current_moment);
	}
	else if(can_id==2)
	{
		ros::param::set("motor2_angle",current_angle);
		ros::param::set("motor2_angular_velocity",current_angular_velocity);
		ros::param::set("motor2_moment",current_moment);
	}
	else if(can_id==3)
	{
		ros::param::set("motor3_angle",current_angle);
		ros::param::set("motor3_angular_velocity",current_angular_velocity);
		ros::param::set("motor3_moment",current_moment);
	}	
	else if(can_id==4)
	{
		ros::param::set("motor4_angle",current_angle);
		ros::param::set("motor4_angular_velocity",current_angular_velocity);
		ros::param::set("motor4_moment",current_moment);
	}
	else if(can_id==5)
	{
		ros::param::set("motor5_angle",current_angle);
		ros::param::set("motor5_angular_velocity",current_angular_velocity);
		ros::param::set("motor5_moment",current_moment);
	}
	else if(can_id==6)
	{
		ros::param::set("motor6_angle",current_angle);
		ros::param::set("motor6_angular_velocity",current_angular_velocity);
		ros::param::set("motor6_moment",current_moment);
	}
}


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"socketcan_do_msgs");

    ROS_INFO("start receive can_messages!");

    ros::NodeHandle nh;

    ros::Subscriber roscan_receive_message = nh.subscribe<can_msgs::Frame>("received_messages",10,do_can_msgs);    

    ros::spin();

    return 0;
}


